#include "OclLapack.h"
#include "../Exceptions.h"
#include <cmath>

using namespace std;

static int c__1 = 1;

//void set(int n, float value, cl_ptr x, int incX) {
//	x += offsetX;
//	for (int i = 0; i < n; i++) {
//		x[i * incX] = value;
//	}
//}
//
//void set(cl_ptr dest, float value) {
//	set(1, value, dest, 1);
//}

//void OclLapack::sorg2r(int m, int n, int k, 
//		cl_ptr a, int lda,
//		const cl_ptr tau,
//		cl_ptr work) 
//{
//	if (m < 0) {
//		throw ArgumentException("sorg2r: m should be >= 0");		
//	} else if (n < 0 || n > m) {
//		throw ArgumentException("sorg2r: n should be >= 0 and <= m");
//	} else if (k < 0 || k > n) {
//		throw ArgumentException("sorg2r: k should be >= 0 and <= n");		
//	} else if (lda < max(1, m)) {
//		throw ArgumentException("sorg2r: lda should be >= 1 and >= m");
//	}
//	
//	// Quick return if possible
//	if (n <= 0) return;
//
//	// Initialise columns k+1:n to columns of the unit matrix	
//	int h = n - k;
//	set(m + (h - 1) * lda, 0, a[k * lda], 1);	
//	set(h + (h - 1) * lda, 1, a[k + k * lda], lda);
//	
//	for (int i = k - 1; i >= 0; --i) {
//		// Apply H(i) to A(i:m,i:n) from the left
//		if (i < n - 1) {
//			set(a[i + i * lda], 1);			
//			int i__1 = m - i;
//			int i__2 = n - i - 1;
//			slarf("Left", m - i, n - i - 1, a[i + i * lda], 1, tau[i], a[i + (i + 1) * lda], lda, work);
//		}
//		
//		if (i < m - 1) {			
//			float r__1 = -tau[i];
//			sscal(m - i - 1, r__1, a[i + 1 + i * lda], 1);
//		}
//		set(a[i + i * lda], 1 - tau[i]);
//				
//		set(i, 0, &a[0], i * lda, 1);
//	}
//}

extern "C" {
    int xerbla_(char *, int *);
}

int OclLapack::sorg2r(int *m, int *n, int *k, float *a,
		int *lda, float *tau, float *work, int *info) 
{
	Measure measure("sorg2r");
		
	/*sorg2r(*m, *n, *k, 
			a, 0, *lda,
			tau, 0, work, 0);*/
	
	*info = 0;
	if (*m < 0) {
		*info = -1;
	} else if (*n < 0 || *n > *m) {
		*info = -2;
	} else if (*k < 0 || *k > *n) {
		*info = -3;
	} else if (*lda < max(1, *m)) {
		*info = -5;
	}
	
	if (*info != 0) {
		int i__1 = -(*info);
		xerbla_("SORG2R", &i__1);
		return 0;
	}

	// Quick return if possible
	if (*n <= 0) {
		return 0;
	}

	// Initialise columns k+1:n to columns of the unit matrix
	for (int j = *k + 1; j <= *n; ++j) {		
		for (int l = 1; l <= *m; ++l) {
			a[-1 - (*lda) + l + j * (*lda)] = 0.f;
		}
		a[-1 - (*lda) + j + j * (*lda)] = 1.f;
	}

	for (int i = *k; i >= 1; --i) {
		// Apply H(i) to A(i:m,i:n) from the left
		if (i < *n) {
			a[-1 - (*lda) + i + i * (*lda)] = 1.f;
			int i__1 = *m - i + 1;
			int i__2 = *n - i;
			slarf("Left", &i__1, &i__2, &a[-1 - (*lda) + i + i * (*lda)], &c__1, &tau[-1 + 
					i], &a[-1 - (*lda) + i + (i + 1) * (*lda)], lda, &work[-1 + 1]);
		}
		if (i < *m) {
			int i__1 = *m - i;
			float r__1 = -tau[-1 + i];
			sscal(&i__1, &r__1, &a[-1 - (*lda) + i + 1 + i * (*lda)], &c__1);
		}
		a[-1 - (*lda) + i + i * (*lda)] = 1.f - tau[-1 + i];

		// Set A(1:i-1,i) to zero		
		for (int l = 1; l <= i - 1; ++l) {
			a[-1 - (*lda) + l + i * (*lda)] = 0.f;
		}		
	}
	return 0;
}
